#include "vision.h"

//构造函数并不会打开摄像头，有特定函数可以随时打开或关闭摄像头
GetLineIFM::GetLineIFM()
{
    Line_DSP = 0.0;
    Line_coordinate_convert = 1;
	Line_LineKind = 0;
	Line_Robot_Center = Point2f(0.0, 0.0);
	Line_TextOut_Center = Point2f(0.0, 0.0);
	Line_Order_Center = Point2f(0.0, 0.0);
	Line_Sort_Center = Point2f(0.0, 0.0);
    Line_KX = 30.0 / 50.0;
    Line_KY = 30.0 / 51.0;
	Line_K_AREA = 1.0;
	Line_Calib_Angel = 0.0;
	Line_Line = Mat::zeros(Size(LINE_SRC_ROI_WIDETH_RESIZE, LINE_SRC_ROI_HEIGHT_RESIZE), CV_8UC1);
	Line_Bin = Mat::zeros(Size(LINE_SRC_ROI_WIDETH_RESIZE, LINE_SRC_ROI_HEIGHT_RESIZE), CV_8UC1);
	Line_SRC_V = Mat::zeros(Size(LINE_SRC_ROI_WIDETH_RESIZE, LINE_SRC_ROI_HEIGHT_RESIZE), CV_8UC1);
	Line_SRC_Color = Mat::zeros(Size(LINE_SRC_ROI_WIDETH_RESIZE, LINE_SRC_ROI_HEIGHT_RESIZE), CV_8UC3);

	Line_X_Y_Swap=0;
	Line_X_Contrary=0;
	Line_Y_Contrary=0;
    switch (Line_coordinate_convert)
	{
	case 1:Line_X_Y_Swap = 0;
		Line_X_Contrary = -1;
		Line_Y_Contrary = 1;
		break;
	case 2:Line_X_Y_Swap = 1;
		Line_X_Contrary = -1;
		Line_Y_Contrary = -1;
		break;
	case 3:Line_X_Y_Swap = 1;
		Line_X_Contrary = 1;
		Line_Y_Contrary = 1;
		break;
	case 4:Line_X_Y_Swap = 0;
		Line_X_Contrary = 1;
		Line_Y_Contrary = 1;
		break;
	default:
		break;
	}
	//CalibIFM();
}

GetLineIFM::~GetLineIFM()
{
	Line_Cap.release();
	CameraMatrix.release();
	DistCoeffs.release();
	Line_Line.release();
}
////////////////////////////////Tools//////////////////////////////////////////
//大津算法，输入8UC1图像
int otsu(Mat src)
{
	int width = src.cols;
	int height = src.rows;
	long size = height * width;

	//histogram    
	float histogram[256] = { 0 };
	for (int m = 0; m < height; m++)
	{
		uchar * p = src.ptr<uchar>(m);
		for (int n = 0; n < width; n++)
		{
			histogram[int(*p++)]++;
		}
	}

	int threshold = 0;
	long sum0 = 0, sum1 = 0; //存储前景的灰度总和和背景灰度总和  
	long cnt0 = 0, cnt1 = 0; //前景的总个数和背景的总个数  
	double w0 = 0, w1 = 0; //前景和背景所占整幅图像的比例  
	double u0 = 0, u1 = 0;  //前景和背景的平均灰度  
	double variance = 0; //最大类间方差  
	int i, j;
	double u = 0;
	double maxVariance = 0;
	for (i = 1; i < 256; i++) //一次遍历每个像素  
	{
		sum0 = 0;
		sum1 = 0;
		cnt0 = 0;
		cnt1 = 0;
		w0 = 0;
		w1 = 0;
		for (j = 0; j < i; j++)
		{
			cnt0 += histogram[j];
			sum0 += j * histogram[j];
		}
		if (cnt0 > 0)
		{
			u0 = (double)sum0 / cnt0;
			w0 = (double)cnt0 / size;
		}
		else
		{
			u0 = 0.0;
			w0 = 0.0;
		}

		for (j = i; j <= 255; j++)
		{
			cnt1 += histogram[j];
			sum1 += j * histogram[j];
		}

		if (cnt1 > 0)
		{
			u1 = (double)sum1 / cnt1;
			w1 = 1 - w0; // (double)cnt1 / size;  
		}
		else
		{
			u1 = 0.0;
			w1 = 0.0;
		}

		u = u0 * w0 + u1 * w1; //图像的平均灰度  
		//printf("u = %f\n", u);
		//variance =  w0 * pow((u0 - u), 2) + w1 * pow((u1 - u), 2);  
		variance = w0 * w1 *  (u0 - u1) * (u0 - u1);
		if (variance > maxVariance)
		{
			maxVariance = variance;
			threshold = i;
		}
	}

	//显示直方图
	int histHeight = 256;
	int intensity_max = 0;
	for (int i = 0; i < 256; i++)
	{
		int k = histogram[i];
		if (k>intensity_max)
			intensity_max = k;
	}
	Mat Hist = Mat::zeros(histHeight, 256, CV_8UC3);
	for (int i = 0; i < 256; i++)
	{
		int intensity = histogram[i];  //要绘制的高度
		intensity = (int)(intensity * 256.0 / intensity_max);
		rectangle(Hist, Point(i, histHeight - 1), Point((i + 1), histHeight - intensity), Scalar(255, 255, 255));
	}
	rectangle(Hist, Point(threshold, histHeight - 1), Point((threshold + 1), 0), Scalar(255, 0, 0));

	//cout << threshold << endl;
	return threshold;

}

void my_split(InputArray _src, OutputArray _dst, int _mode)
{
	Mat src_HSV;
	vector<Mat> channels;
	switch (_mode)
	{
	case GET_B:
		split(_src, channels);
		channels.at(0).copyTo(_dst);
		break;
	case GET_G:
		split(_src, channels);
		channels.at(1).copyTo(_dst);
		break;
	case GET_R:
		split(_src, channels);
		channels.at(2).copyTo(_dst);
		break;
	case GET_H:
		cvtColor(_src, src_HSV, CV_BGR2HSV);
		split(src_HSV, channels);
		channels.at(0).copyTo(_dst);
		break;
	case GET_S:
		cvtColor(_src, src_HSV, CV_BGR2HSV);
		split(src_HSV, channels);
		channels.at(1).copyTo(_dst);
		break;
	case GET_V:
		cvtColor(_src, src_HSV, CV_BGR2HSV);
		split(src_HSV, channels);
		channels.at(2).copyTo(_dst);
		break;
	default:
		break;
	}
}


//得到标定参数
void GetLineIFM::CalibIFM()
{
	FileStorage fs("params/zhen120.xml", FileStorage::READ);
	fs["cameraMatrix"] >> CameraMatrix;
	fs["cameraDistcoeff"] >> DistCoeffs;
}
//////////////////////////////////////////////////////////////////////////



void GetLineIFM::DoInit()
{
	//需要一定的时间进行曝光
	for (int i = 0; i < 20; i++)
		GetLineContour();
	GetCenter();
	InitAngel();
	/*Mat src, srcS;
	coloursImage.copyTo(src);
	cvtColor(src, src, CV_BGR2HSV);
	vector<Mat>channels;
	cv::split(src, channels);
	channels.at(1).copyTo(srcS);
	srcS = srcS>200;*/
	/*	Mat src;
		grayImage.copyTo(src);
		src = src > 250;
		imshow("1", src);
		imshow("2", grayImage);
		waitKey(1);
		*/
	//显示问题，官方解释是：highgui里并没有给imshow绘制时间，需要一个waitkey(1);
	//很关键*****************
}

//得到满足条件的轮廓，存于ContourOfLine
//判断线的种类
//lineKind;
// 0：no line contour 1:摄像头横线
// 2：摄像头竖线 3: 横竖线均有
//运行完可得灰度图，line二值图,所有轮廓图，linekind
int GetLineIFM::GetLineContour()
{
	Mat src;
	if (Line_Cap.isOpened())
	{
		Line_Cap >> src;

		Mat src_ROI(src, Rect(70, 0, LINE_SRC_ROI_WIDETH, LINE_SRC_ROI_HEIGHT));
		Mat src_ROI_Resize;
		resize(src_ROI, src_ROI_Resize, Size(LINE_SRC_ROI_WIDETH_RESIZE, LINE_SRC_ROI_HEIGHT_RESIZE));
        src_ROI_Resize.copyTo(Line_Src);
        Mat src_U8_S, Bin_S, src_U8_V;

		my_split(src_ROI_Resize, src_U8_S, GET_S);
		my_split(src_ROI_Resize, src_U8_V, GET_V);

		//为方便调试而保存关键的图——类的变量里
		src.copyTo(Line_SRC_Color);
		src_U8_V.copyTo(Line_SRC_V);
        src_U8_S.copyTo(Line_BeThreshold);
		//保存完后，对需要的图像进行处理		
		Bin_S = src_U8_S < 60;

		//处理完照常存下，方便类的其他函数调用
		Bin_S.copyTo(Line_Bin);//存下

		//获取到输入二值图像后，进行轮廓处理
		vector<vector<Point> > contours;
		vector<Vec4i> hierarchy;
		double radius = 0.0;//为十字准备的变量
		int number = -1;
		Mat img2contours;
		Bin_S.copyTo(img2contours);//findcontours函数会改变输入图像，因此copy使用
		findContours(img2contours, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
		//分析每一个检测到的轮廓，走到条件最后一步的轮廓定为线轮廓
		//先不用顾及错误情况时令linekind=0，number会管理这件事;
		int contoursSize = contours.size();
		for (int i = 0; i<contoursSize; i++)
		{
			double nowarea = contourArea(contours[i]);
			if (nowarea > 10000)//初步筛选，仅剩比较大的轮廓
			{
				Mat choose_contour = Mat::zeros(src_U8_S.size(), CV_8UC1);
				drawContours(choose_contour, contours, i, Scalar(255), -1);//画轮廓
				//使用canny算法，利用霍夫线变换辅助筛选线轮廓
				Mat choose_forCanny;
				blur(choose_contour, choose_contour, Size(7, 7));
				choose_contour = choose_contour > 1;
				choose_contour.copyTo(choose_forCanny);
				Canny(choose_forCanny, choose_forCanny, 3, 9, 3);

				//线变换存储变量定义
				float angel = 0.0;
				int l_0, l_1;//两条线才确定有线轮廓
				int x = 0, y = 0;
				float k1 = 0, k2 = 0, b1 = 0, b2 = 0;//用于得到解决特殊情况的目标点的坐标
				//用于prepareDstCenter;
				vector<Vec4i> lines;//定义一个矢量结构lines用于存放得到的线段矢量集合
				HoughLinesP(choose_forCanny, lines, 1, CV_PI / 180, 50, 50, 200);
				// 注意100，200的意思

				//依次分析每条线
				int linesSize = lines.size();
				for (int j = 0; j < linesSize; j++)
				{
					Vec4i l = lines[j];
					if (j < 100)
					{
						//直接计算该线的角度
						//注意：像素为整数所以要得浮点型数据时，得乘个小数
						angel = atan(((l[0] - l[2])*0.1) / ((l[1] - l[3])*0.1)) / CV_PI * 180.0;
						if (abs(angel) < 30)//竖直偏角
						{
							
							if ((l[0] < 2.0 && l[2]<2.0) || (l[0]>LINE_SRC_ROI_WIDETH_RESIZE-3 && l[2] > LINE_SRC_ROI_WIDETH_RESIZE-3))
							{
								continue;
							}
							else
							{
								
								if (y == 0)
								{
									l_0 = min(l[0],l[2]);
									y++;
								}
								else if (abs(min(l[0],l[2]) - l_0) > 15)
								{
									y++;
								}

								if (min(l[0], l[2]) <= l_0)
								{
									if (l[0] == l[2])
									{
										k1 = 998;//处理竖直线。
										b1 = l[0];
									}
									else
									{
										k1 = ((l[1] - l[3]) * 0.1) / ((l[0] - l[2]) * 0.1);
										b1 = l[1] - k1*l[0];
									}
								}
																
							}

						}
						else if ((abs(angel) - 60) < 31 && (abs(angel) - 60) > 0)//横线
						{
							if ((l[1] < 2.0 && l[3]<2.0) || (l[1]>LINE_SRC_ROI_HEIGHT_RESIZE-2 && l[3] > LINE_SRC_ROI_HEIGHT_RESIZE-2))
							{
								continue;
							}
							else
							{
								if (x == 0)
								{
									l_1 = min(l[1],l[3]);
									x++;
								}
								else if (abs(min(l[1],l[3]) - l_1) > 15)
								{
									x++;
								}

								if (min(l[1], l[3]) <= l_1)
								{
									k2 = ((l[1] - l[3]) * 0.1) / ((l[0] - l[2]) * 0.1);
									b2 = l[1] - k2 * l[0];
								}					
							}

						}
					}
				}

				if (x != 0 || y != 0)
				{
					//霍夫线判断通过后
					Point2f _center;
					float _radius;
					minEnclosingCircle(contours[i], _center, _radius);
					if (nowarea / (CV_PI*_radius*_radius) < Line_K_AREA)
					{
						if (x && y && _radius> radius)//用最小圆包围 挑半径大的轮廓
						{
							radius = _radius;
							Line_LineKind = 3;
							number = i;
							//存储prepareDstCenter
							if ((int)k1 == 998 && x != 0)
							{
								Line_Sort_Center.x = b1;
								Line_Sort_Center.y = k2*Line_Sort_Center.x + b2;

							}
							else if (y != 0 && x != 0)
							{
								Line_Sort_Center.x = (b2 - b1) / (k1 - k2);
								Line_Sort_Center.y = k2*Line_Sort_Center.x + b2;
							}
						}
						else if (!x && (y > 1) /*&& nowarea < 52000*/)
						{
							//竖向线
							Line_LineKind = 2;
							number = i;
							break;
						}
						else if ((x > 1) && !y /*&& nowarea < 52000*/)
						{
							//横向线
							Line_LineKind = 1;
							number = i;
							break;
							//only one line

						}
						else
						{

						}
					}
				}
			}

		}
		//final为图像滤波最后完成图，后面的工作是数据处理
		Mat final = Mat::zeros(src_U8_S.size(), CV_8UC1);
		if (number != -1)//当有满足条件的轮廓时
		{
			drawContours(final, contours, number, Scalar(255), -1);

			Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
			//进行形态学操作
			morphologyEx(final, final, MORPH_CLOSE, element);
			final.copyTo(Line_Line);//线的廓图像保存到ContourOfLine 以备计算角度函数使用
			return 1;
		}
		else
		{
			Line_LineKind = 0;//no line contour
            return 0;
		}
	}
	//摄像头未打开
	else
	{
		Line_LineKind = 0;
		return 0;
	}
}


//根据线的种类进行对应的计算，并保存值到公共数组中
//反正return 0;就不用理会这个函数的了，不去读IFM[]中的值了。
//return 0；表示虽然检测到有效霍夫线，但是轮廓却不满足要求，无法处理出有效信息
int GetLineIFM::Calculate(float IFM[])
{
    switch (Line_coordinate_convert)
    {
    case 1:Line_X_Y_Swap = 0;
        Line_X_Contrary = -1;
        Line_Y_Contrary = 1;
        break;
    case 2:Line_X_Y_Swap = 1;
        Line_X_Contrary = -1;
        Line_Y_Contrary = -1;
        break;
    case 3:Line_X_Y_Swap = 1;
        Line_X_Contrary = 1;
        Line_Y_Contrary = 1;
        break;
    case 4:Line_X_Y_Swap = 0;
        Line_X_Contrary = 1;
        Line_Y_Contrary = 1;
        break;
    default:
        break;
    }
	if (Line_LineKind > 0)
	{
        if (Line_LineKind == 1)
		{
			//处理横向线
			Mat forCalculat;
			Line_Line.copyTo(forCalculat);
			float angel = 0, deltaY = 0;
			Mat left(forCalculat, Rect(0, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE)), right(forCalculat, Rect(LINE_SRC_ROI_WIDETH_RESIZE-LINE_LR_ROI_WIDETH, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE));

			vector<vector<Point> > leftcontour;
			vector<Vec4i> lefthierarchy;
			findContours(left, leftcontour, lefthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
			vector<vector<Point> > rightcontour;
			vector<Vec4i> righthierarchy;
			findContours(right, rightcontour, righthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

			//确保只有一个轮廓
			int leftcontourSize = leftcontour.size();
			int rightcontourSize = rightcontour.size();
			if (leftcontourSize == 1 && rightcontourSize == 1)
			{
				vector<Moments> muleft(1);
				Point2f mcleft;
				muleft[0] = moments(leftcontour[0], false);
				mcleft = Point2f(static_cast<float>(muleft[0].m10 / muleft[0].m00), static_cast<float>(muleft[0].m01 / muleft[0].m00));

				vector<Moments> muright(1);
				Point2f mcright;
				muright[0] = moments(rightcontour[0], false);
				mcright = Point2f(static_cast<float>(muright[0].m10 / muright[0].m00), static_cast<float>(muright[0].m01 / muright[0].m00));

				if (mcleft.y == mcright.y)
				{
					//水平情况;
					deltaY = (Line_Robot_Center.y - mcleft.y) * Line_KY;
					Line_TextOut_Center = Point2f(Line_Robot_Center.x, mcleft.y);//更新目标坐标点
					Line_Order_Center.y = Line_TextOut_Center.y;
					
					OutPutIFM(IFM, 0.0, deltaY);
					return 1;
				}
				else
				{
					//非平行
					float k1, b1;
					k1 = (mcleft.y - mcright.y) / (mcleft.x - (mcright.x + LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH));
					b1 = (mcleft.y - k1*mcleft.x);

					/*float k2, b2;
					k2 = -1 / k1;
					b2 = center.y - k2*center.x;
					int x = (b2 - b1) / (k1 - k2);
					int y = k1*x + b1;*/					
					deltaY = (Line_Robot_Center.y - (k1*Line_Robot_Center.x+b1)) * Line_KY;
					Line_TextOut_Center = Point2f(Line_Robot_Center.x, (k1*Line_Robot_Center.x + b1));
					Line_Order_Center.y = Line_TextOut_Center.y;

					OutPutIFM(IFM, 0.0, deltaY);						
					angel = atan(k1) / CV_PI * 180;
					IFM[3] = angel;
					return 1;
				}
			}
			return 0;
		}
		else if (Line_LineKind == 2)
		{
			//竖向线
			Mat forCalculat;
			Line_Line.copyTo(forCalculat);
			float angel = 0, deltaX = 0;
			Mat up(forCalculat, Rect(0, 0, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT)), down(forCalculat, Rect(0, LINE_SRC_ROI_HEIGHT_RESIZE-LINE_UD_ROI_HEIGHT, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT));
			vector<vector<Point> > upcontour;
			vector<Vec4i> uphierarchy;
			findContours(up, upcontour, uphierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
			vector<vector<Point> > downcontour;
			vector<Vec4i> downhierarchy;
			findContours(down, downcontour, downhierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
			//确保兴趣区只有一个轮廓
			int upcontourSize = upcontour.size();
			int downcontourSize = downcontour.size();
			if (upcontourSize == 1 && downcontourSize == 1)
			{
				vector<Moments> muup(1);
				Point2f mcup;
				muup[0] = moments(upcontour[0], false);
				mcup = Point2f(static_cast<float>(muup[0].m10 / muup[0].m00), static_cast<float>(muup[0].m01 / muup[0].m00));

				vector<Moments> mudown(1);
				Point2f mcdown;
				mudown[0] = moments(downcontour[0], false);
				mcdown = Point2f(static_cast<float>(mudown[0].m10 / mudown[0].m00), static_cast<float>(mudown[0].m01 / mudown[0].m00));

				//竖直线 y=k1*x+b1

				float k1 = 0;
				float b1 = 0;

				if (mcup.x == mcdown.x)
				{
					//竖直情况;
					angel = 0.0;
					deltaX = (Line_Robot_Center.x - mcup.x) * Line_KX;
					Line_TextOut_Center = Point2f(mcup.x, Line_Robot_Center.y);
					Line_Order_Center.x = Line_TextOut_Center.x;
					OutPutIFM(IFM, deltaX, 0.0);
					IFM[3] = angel - Line_Calib_Angel;
					return 1;
				}
				else
				{
					//非竖直情况;
					k1 = (mcup.y - (mcdown.y + LINE_SRC_ROI_HEIGHT_RESIZE-LINE_UD_ROI_HEIGHT)) / (mcup.x - mcdown.x);
					b1 = (mcup.y - k1*mcup.x);

					angel = atan(k1) / CV_PI * 180;
					deltaX = (Line_Robot_Center.x - (Line_Robot_Center.y - b1) / k1) * Line_KX;
					Line_TextOut_Center = Point2f((Line_Robot_Center.y - b1) / k1, Line_Robot_Center.y);
					Line_Order_Center.x = Line_TextOut_Center.x;
					IFM[0] = 0; //纯直线状态
					/*IFM[1] = -deltaX;*/
					OutPutIFM(IFM, deltaX, 0.0);
					if (angel < 0)
						IFM[3] = angel + 90.0 - Line_Calib_Angel;
					else
						IFM[3] = angel - 90.0 - Line_Calib_Angel;
					return 1;
				}
			}
			return 0;
		}
		else if (Line_LineKind == 3)
		{		
            if (abs(Line_Robot_Center.x - Line_Sort_Center.x) < 22 / Line_KX&&abs(Line_Robot_Center.y - Line_Sort_Center.y) < 22 / Line_KY)
			{
				_up = 0;
				_down = LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT;
				_left = 0;
				_right = LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH;
				Mat forCalculat;
				Line_Line.copyTo(forCalculat);
				Point2f thisCenter = Point2f(0.0, 0.0);
				float angel = 0, deltaX = 0, deltaY = 0;
				Mat up(forCalculat, Rect(0, _up, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT)), down(forCalculat, Rect(0, _down, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT));
				Mat left(forCalculat, Rect(_left, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE)), right(forCalculat, Rect(_right, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE));

				Mat forContour_up, forContour_down, forContour_left, forContour_right;
				up.copyTo(forContour_up);
				down.copyTo(forContour_down);
				left.copyTo(forContour_left);
				right.copyTo(forContour_right);
				vector<vector<Point> > upcontour;
				vector<Vec4i> uphierarchy;
				findContours(forContour_up, upcontour, uphierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
				vector<vector<Point> > downcontour;
				vector<Vec4i> downhierarchy;
				findContours(forContour_down, downcontour, downhierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
				vector<vector<Point> > leftcontour;
				vector<Vec4i> lefthierarchy;
				findContours(forContour_left, leftcontour, lefthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
				vector<vector<Point> > rightcontour;
				vector<Vec4i> righthierarchy;
				findContours(forContour_right, rightcontour, righthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

				//确保只有一个轮廓
				int upcontourSize = upcontour.size();
				int downcontourSize = downcontour.size();
				int leftcontourSize = leftcontour.size();
				int rightcontourSize = rightcontour.size();
				if (upcontourSize == 1 && downcontourSize == 1 && leftcontourSize == 1 && rightcontourSize == 1)
				{

					if ((contourArea(upcontour[0]) > 300 && contourArea(downcontour[0]) > 300 && contourArea(leftcontour[0]) > 300 && contourArea(rightcontour[0]) > 300))
					{
						vector<Moments> muup(1);
						Point2f mcup;
						muup[0] = moments(upcontour[0], false);
						mcup = Point2f(static_cast<float>(muup[0].m10 / muup[0].m00), static_cast<float>(muup[0].m01 / muup[0].m00));

						vector<Moments> mudown(1);
						Point2f mcdown;
						mudown[0] = moments(downcontour[0], false);
						mcdown = Point2f(static_cast<float>(mudown[0].m10 / mudown[0].m00), static_cast<float>(mudown[0].m01 / mudown[0].m00));

						vector<Moments> muleft(1);
						Point2f mcleft;
						muleft[0] = moments(leftcontour[0], false);
						mcleft = Point2f(static_cast<float>(muleft[0].m10 / muleft[0].m00), static_cast<float>(muleft[0].m01 / muleft[0].m00));

						vector<Moments> muright(1);
						Point2f mcright;
						muright[0] = moments(rightcontour[0], false);
						mcright = Point2f(static_cast<float>(muright[0].m10 / muright[0].m00), static_cast<float>(muright[0].m01 / muright[0].m00));

						mcup = Point2f(mcup.x, mcup.y + _up);
						mcdown = Point2f(mcdown.x, mcdown.y + _down);
						mcleft = Point2f(mcleft.x + _left, mcleft.y);
						mcright = Point2f(mcright.x + _right, mcright.y);

						//竖直线 y=k1*x+b1
						//横线   y=k2*x+b2

						float k1 = 0, k2 = 0;
						float b1 = 0, b2 = 0;

						if (mcup.x == mcdown.x)
						{
							//90°垂直
							thisCenter.x = mcup.x;
						}
						else
						{
							k1 = (mcup.y - mcdown.y) / (mcup.x - mcdown.x);
							b1 = (mcup.y - k1*mcup.x);
						}

						if (mcleft.y == mcright.y)
						{
							//水平
							thisCenter.y = mcleft.y;
							k2 = 0;
							b2 = thisCenter.y;
						}
						else if (mcleft.x != mcright.x)//不可能相等
						{
							k2 = (mcleft.y - mcright.y) / (mcleft.x - mcright.x);
							b2 = (mcleft.y - k2*mcleft.x);
						}
						else
						{
							return 1;
						}
						//k1==0 则表示angel==0;center.x = mcup.x;
						if (k1 != 0 && k1 != k2)
						{
							thisCenter.x = (b2 - b1) / (k1 - k2);
							thisCenter.y = k1*thisCenter.x + b1;
						}
						else if (mcleft.y != mcright.y)//若center.y 还未赋值，即k2!=0,已求得k2,b2;否则不用赋值了
													   //因为上方代码已经给它赋值了
						{
							thisCenter.y = k2*thisCenter.x + b2;
						}
						angel = atan(k1) / CV_PI * 180;
						deltaX = (Line_Robot_Center.x - thisCenter.x) * Line_KX;
						deltaY = (Line_Robot_Center.y - thisCenter.y) * Line_KY;
						Line_TextOut_Center = Point2f(thisCenter.x, thisCenter.y);
						Line_Order_Center.x = Line_TextOut_Center.x;
						Line_Order_Center.y = Line_TextOut_Center.y;
						OutPutIFM(IFM, deltaX, deltaY);

						return 1;
					}
				}
				return 1;
			}
			else {
				if (Line_Robot_Center.x < Line_Sort_Center.x&&Line_Robot_Center.y < Line_Sort_Center.y) {
					_up = 0;
					_down = LINE_UD_ROI_HEIGHT + LINE_CHANGEROI_DIS;
					_left = 0;
					_right = LINE_LR_ROI_WIDETH + LINE_CHANGEROI_DIS;
					Calculate_ChangeROI(IFM, 1);
				}
				else if (Line_Robot_Center.x > Line_Sort_Center.x&&Line_Robot_Center.y < Line_Sort_Center.y) {
					_up = 0;
					_down = LINE_UD_ROI_HEIGHT + LINE_CHANGEROI_DIS;
					_left = LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH - LINE_CHANGEROI_DIS;
					_right = LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH;
					Calculate_ChangeROI(IFM, 2);
				}
				else if (Line_Robot_Center.x > Line_Sort_Center.x&&Line_Robot_Center.y > Line_Sort_Center.y) {
					_up = LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT - LINE_CHANGEROI_DIS;
					_down = LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT;
					_left = LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH - LINE_CHANGEROI_DIS;
					_right = LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH;
					Calculate_ChangeROI(IFM, 3);
				}
				else {
					_up = LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT - LINE_CHANGEROI_DIS;
					_down = LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT;
					_left = 0;
					_right = LINE_LR_ROI_WIDETH + LINE_CHANGEROI_DIS;
					Calculate_ChangeROI(IFM,4);
				}
				return 1;
			}		
		}
		else
		{
			//重置IFM
			for (int i = 0; i < 4; i++)
			{
				IFM[i] = 0;
			}
			return 0;
		}
	}
	else
	{
		//lineKind = 0;不更改IFM值
		/*dstCenter = Point2f(0.0, 0.0);*/
		return 0;
	}
}

int GetLineIFM::TextShow(int _mode, Mat& dst)
{
    Mat image,_imageU8;
	Line_Line.copyTo(image);
	//输文本
	char buf1[256];
	sprintf_s(buf1, 256, "lineKind: %d", Line_LineKind);
	char buf2[256];
	sprintf_s(buf2, 256, "startAngel: %f", Line_Calib_Angel);

	switch (_mode)
	{
	case LINE_SHOW_LINE://线轮廓图
		line(image, Point(0, LINE_UD_ROI_HEIGHT), Point(LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT), Scalar(100), 2);
		line(image, Point(LINE_LR_ROI_WIDETH, 0), Point(LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE), Scalar(100), 2);
		line(image, Point(0, LINE_SRC_ROI_HEIGHT_RESIZE-LINE_UD_ROI_HEIGHT), Point(LINE_SRC_ROI_WIDETH_RESIZE, LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT), Scalar(100), 2);
		line(image, Point(LINE_SRC_ROI_WIDETH_RESIZE-LINE_LR_ROI_WIDETH, 0), Point(LINE_SRC_ROI_WIDETH_RESIZE - LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE), Scalar(100), 2);
		putText(image, buf1, Point(20, 40), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(100), 1);
		putText(image, buf2, Point(20, 80), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(100), 1);
		image.copyTo(dst);
		return 1;
	case LINE_SHOW_BIN://所有轮廓
		Line_Bin.copyTo(dst);
		return 1;
	case LINE_SHOW_V://灰度图,附加原始机械中心(激光标的),和目标中心
        Line_SRC_V.copyTo(_imageU8);
        circle(_imageU8, Line_Robot_Center, 7, Scalar(100), 2);
        circle(_imageU8, Line_Order_Center, 7, Scalar(50), 2);
        _imageU8.copyTo(dst);
		return 1;
	case LINE_SHOW_SRCCOLOR:
		Line_SRC_Color.copyTo(dst);
		return 1;
	case LINE_SHOW_BETHRESHOLD:
		Line_BeThreshold.copyTo(dst);
		return 1;
    case LINE_SHOW_SRC:
        Line_Src.copyTo(dst);
        return 1;
	default:
		dst = Mat(Line_SRC_Color.size(), CV_8UC3, Scalar(255, 0, 0));
		return 0;
	}
}

void GetLineIFM::Calculate_ChangeROI(float* _IFM,int _mode)
{
	//兴趣区域已改变
	Mat forCalculat;
	Line_Line.copyTo(forCalculat);
	Point2f thisCenter = Point2f(0.0, 0.0);
	float angel = 0, deltaX = 0, deltaY = 0;
	Mat up(forCalculat, Rect(0, _up, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT)), down(forCalculat, Rect(0, _down, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT));
	Mat left(forCalculat, Rect(_left, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE)), right(forCalculat, Rect(_right, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE));

	Mat forContour_up, forContour_down, forContour_left, forContour_right;
	up.copyTo(forContour_up);
	down.copyTo(forContour_down);
	left.copyTo(forContour_left);
	right.copyTo(forContour_right);
	vector<vector<Point> > upcontour;
	vector<Vec4i> uphierarchy;
	findContours(forContour_up, upcontour, uphierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
	vector<vector<Point> > downcontour;
	vector<Vec4i> downhierarchy;
	findContours(forContour_down, downcontour, downhierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
	vector<vector<Point> > leftcontour;
	vector<Vec4i> lefthierarchy;
	findContours(forContour_left, leftcontour, lefthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
	vector<vector<Point> > rightcontour;
	vector<Vec4i> righthierarchy;
	findContours(forContour_right, rightcontour, righthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

	//确保只有一个轮廓
	int upcontourSize = upcontour.size();
	int downcontourSize = downcontour.size();
	int leftcontourSize = leftcontour.size();
	int rightcontourSize = rightcontour.size();
	if (upcontourSize == 1 && downcontourSize == 1 && leftcontourSize == 1 && rightcontourSize == 1)
	{
		if ((contourArea(upcontour[0]) > 200 && contourArea(downcontour[0]) > 200 && contourArea(leftcontour[0]) > 200 && contourArea(rightcontour[0]) > 200))
		{
			vector<Moments> muup(1);
			Point2f mcup;
			muup[0] = moments(upcontour[0], false);
			mcup = Point2f(static_cast<float>(muup[0].m10 / muup[0].m00), static_cast<float>(muup[0].m01 / muup[0].m00));

			vector<Moments> mudown(1);
			Point2f mcdown;
			mudown[0] = moments(downcontour[0], false);
			mcdown = Point2f(static_cast<float>(mudown[0].m10 / mudown[0].m00), static_cast<float>(mudown[0].m01 / mudown[0].m00));

			vector<Moments> muleft(1);
			Point2f mcleft;
			muleft[0] = moments(leftcontour[0], false);
			mcleft = Point2f(static_cast<float>(muleft[0].m10 / muleft[0].m00), static_cast<float>(muleft[0].m01 / muleft[0].m00));

			vector<Moments> muright(1);
			Point2f mcright;
			muright[0] = moments(rightcontour[0], false);
			mcright = Point2f(static_cast<float>(muright[0].m10 / muright[0].m00), static_cast<float>(muright[0].m01 / muright[0].m00));

			mcup = Point2f(mcup.x, mcup.y + _up);
			mcdown = Point2f(mcdown.x, mcdown.y + _down);
			mcleft = Point2f(mcleft.x + _left, mcleft.y);
			mcright = Point2f(mcright.x + _right, mcright.y);

			//竖直线 y=k1*x+b1
			//横线   y=k2*x+b2

			float k1 = 0, k2 = 0;
			float b1 = 0, b2 = 0;

			if (mcup.x == mcdown.x)
			{
				//90°垂直
				thisCenter.x = mcup.x;
			}
			else
			{
				k1 = (mcup.y - mcdown.y) / (mcup.x - mcdown.x);
				b1 = (mcup.y - k1*mcup.x);
			}

			if (mcleft.y == mcright.y)
			{
				//水平
				thisCenter.y = mcleft.y;
				k2 = 0;
				b2 = thisCenter.y;
			}
			else if (mcleft.x != mcright.x)//不可能相等
			{
				k2 = (mcleft.y - mcright.y) / (mcleft.x - mcright.x);
				b2 = (mcleft.y - k2*mcleft.x);
			}
			else
			{
				return;
			}
			//k1==0 则表示angel==0;center.x = mcup.x;
			if (k1 != 0 && k1 != k2)
			{
				thisCenter.x = (b2 - b1) / (k1 - k2);
				thisCenter.y = k1*thisCenter.x + b1;
			}
			else if (mcleft.y != mcright.y)//若center.y 还未赋值，即k2!=0,已求得k2,b2;否则不用赋值了
										   //因为上方代码已经给它赋值了
			{
				thisCenter.y = k2*thisCenter.x + b2;
			}
			angel = atan(k1) / CV_PI * 180;
			deltaX = (Line_Robot_Center.x - thisCenter.x) * Line_KX;
			deltaY = (Line_Robot_Center.y - thisCenter.y) * Line_KY;
			Line_TextOut_Center = Point2f(thisCenter.x, thisCenter.y);
			Line_Order_Center.x = Line_TextOut_Center.x;
			Line_Order_Center.y = Line_TextOut_Center.y;
			OutPutIFM(_IFM, deltaX, deltaY);

			return;
		}
		else
		{
			Calulate_HL(_IFM, _mode,upcontour[0],downcontour[0],leftcontour[0],rightcontour[0]);
			return;
		}
	}
	else
	{
		Calulate_HL_SIZE(_IFM, _mode, upcontourSize, downcontourSize, leftcontourSize, rightcontourSize);
		return;
	}	
}

void GetLineIFM::Calulate_HL(float* _IFM, int _mode, vector<Point> upcontour, vector<Point> downcontour, vector<Point> leftcontour, vector<Point> rightcontour)
{
	Point2f thisCenter = Point2f(0.0, 0.0);
	float k1 = 0, k2 = 0;
	float b1 = 0, b2 = 0;
	float deltaX = 0.0 , deltaY =0.0;
	if (contourArea(upcontour) > 200 && contourArea(downcontour) > 200)
	{
		
		vector<Moments> muup(1);
		Point2f mcup;
		muup[0] = moments(upcontour, false);
		mcup = Point2f(static_cast<float>(muup[0].m10 / muup[0].m00), static_cast<float>(muup[0].m01 / muup[0].m00));

		vector<Moments> mudown(1);
		Point2f mcdown;
		mudown[0] = moments(downcontour, false);
		mcdown = Point2f(static_cast<float>(mudown[0].m10 / mudown[0].m00), static_cast<float>(mudown[0].m01 / mudown[0].m00));

		mcup = Point2f(mcup.x, mcup.y + _up);
		mcdown = Point2f(mcdown.x, mcdown.y + _down);
		/*mcleft = Point2f(mcleft.x + _left, mcleft.y);
		mcright = Point2f(mcright.x + _right, mcright.y);*/

		if (mcup.x == mcdown.x)
		{
			//90°
			thisCenter.x = mcup.x;
		}
		else
		{
			k1 = (mcup.y - mcdown.y) / (mcup.x - mcdown.x);
			b1 = (mcup.y - k1*mcup.x);
		}
	}
	else
	{
		if (_mode == 1||_mode==4) {
			thisCenter.x = LINE_SRC_ROI_WIDETH_RESIZE - 0.5 * LINE_LR_ROI_WIDETH;
		}		
		else{
			thisCenter.x = 0.5 * LINE_LR_ROI_WIDETH;
		}
	}
	if (contourArea(leftcontour) > 200 && contourArea(rightcontour) > 200)
	{		
		vector<Moments> muleft(1);
		Point2f mcleft;
		muleft[0] = moments(leftcontour, false);
		mcleft = Point2f(static_cast<float>(muleft[0].m10 / muleft[0].m00), static_cast<float>(muleft[0].m01 / muleft[0].m00));

		vector<Moments> muright(1);
		Point2f mcright;
		muright[0] = moments(rightcontour, false);
		mcright = Point2f(static_cast<float>(muright[0].m10 / muright[0].m00), static_cast<float>(muright[0].m01 / muright[0].m00));
		
		/*mcup = Point2f(mcup.x, mcup.y + _up);
		mcdown = Point2f(mcdown.x, mcdown.y + _down);*/
		mcleft = Point2f(mcleft.x + _left, mcleft.y);
		mcright = Point2f(mcright.x + _right, mcright.y);
		if (mcleft.y == mcright.y)
		{
			//水平
			thisCenter.y = mcleft.y;
			k2 = 0;
			b2 = thisCenter.y;
		}
		else if (mcleft.x != mcright.x)//不可能相等
		{
			k2 = (mcleft.y - mcright.y) / (mcleft.x - mcright.x);
			b2 = (mcleft.y - k2*(mcleft.x));
		}
		else
		{
			return;
		}
	}
	else {
		if (_mode <= 2)
		{
			thisCenter.y = LINE_SRC_ROI_HEIGHT_RESIZE - 0.5*LINE_UD_ROI_HEIGHT;
			k2 = 0;
			b2 = thisCenter.y;
		}
		else {
			thisCenter.y = 0.5*LINE_UD_ROI_HEIGHT;
			k2 = 0;
			b2 = thisCenter.y;
		}
	}
	if (k1){
		thisCenter.x = (b2 - b1) / (k1 - k2);
		thisCenter.y = k1*thisCenter.x + b1;		
	}
	else {
		thisCenter.y = k2*thisCenter.x + b2;
	}


	deltaX = (Line_Robot_Center.x - thisCenter.x) * Line_KX;
	deltaY = (Line_Robot_Center.y - thisCenter.y) * Line_KY;
	Line_TextOut_Center = Point2f(thisCenter.x, thisCenter.y);
	Line_Order_Center.x = Line_TextOut_Center.x;
	Line_Order_Center.y = Line_TextOut_Center.y;
	OutPutIFM(_IFM, deltaX, deltaY);
	return;
}

void GetLineIFM::Calulate_HL_SIZE(float* _IFM, int _mode, int upcontour_size, int downcontour_size, int leftcontour_size, int rightcontour_size)
{
	Point2f thisCenter = Point2f(0.0, 0.0);
	float k1 = 0, k2 = 0;
	float b1 = 0, b2 = 0;
	float deltaX = 0.0, deltaY = 0.0;
	if (upcontour_size == 1 && downcontour_size == 1)
	{
		Mat forCalculat;
		Line_Line.copyTo(forCalculat);
		Mat up(forCalculat, Rect(0, _up, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT)), down(forCalculat, Rect(0, _down, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT));

		Mat forContour_up, forContour_down;
		up.copyTo(forContour_up);
		down.copyTo(forContour_down);

		vector<vector<Point> > upcontour;
		vector<Vec4i> uphierarchy;
		findContours(forContour_up, upcontour, uphierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
		vector<vector<Point> > downcontour;
		vector<Vec4i> downhierarchy;
		findContours(forContour_down, downcontour, downhierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
		
		if (contourArea(upcontour[0]) > 200 && contourArea(downcontour[0]) > 200)
		{

			vector<Moments> muup(1);
			Point2f mcup;
			muup[0] = moments(upcontour[0], false);
			mcup = Point2f(static_cast<float>(muup[0].m10 / muup[0].m00), static_cast<float>(muup[0].m01 / muup[0].m00));

			vector<Moments> mudown(1);
			Point2f mcdown;
			mudown[0] = moments(downcontour[0], false);
			mcdown = Point2f(static_cast<float>(mudown[0].m10 / mudown[0].m00), static_cast<float>(mudown[0].m01 / mudown[0].m00));

			mcup = Point2f(mcup.x, mcup.y + _up);
			mcdown = Point2f(mcdown.x, mcdown.y + _down);
			/*mcleft = Point2f(mcleft.x + _left, mcleft.y);
			mcright = Point2f(mcright.x + _right, mcright.y);*/

			if (mcup.x == mcdown.x)
			{
				//90°
				thisCenter.x = mcup.x;
			}
			else
			{
				k1 = (mcup.y - mcdown.y) / (mcup.x - mcdown.x);
				b1 = (mcup.y - k1*mcup.x);
			}
		}
		else
		{
			if (_mode == 1 || _mode == 4) {
				thisCenter.x = LINE_SRC_ROI_WIDETH_RESIZE - 0.5 * LINE_LR_ROI_WIDETH;
			}
			else {
				thisCenter.x = 0.5 * LINE_LR_ROI_WIDETH;
			}
		}
	}
	else
	{
		if (_mode == 1 || _mode == 4) {
			thisCenter.x = LINE_SRC_ROI_WIDETH_RESIZE - 0.5 * LINE_LR_ROI_WIDETH;
		}
		else {
			thisCenter.x = 0.5 * LINE_LR_ROI_WIDETH;
		}
	}
	if (leftcontour_size == 1 && rightcontour_size == 1)
	{
		Mat forCalculat;
		Line_Line.copyTo(forCalculat);
		Mat left(forCalculat, Rect(_left, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE)), right(forCalculat, Rect(_right, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT_RESIZE));

		Mat forContour_left, forContour_right;
		left.copyTo(forContour_left);
		right.copyTo(forContour_right);

		vector<vector<Point> > leftcontour;
		vector<Vec4i> lefthierarchy;
		findContours(forContour_left, leftcontour, lefthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
		vector<vector<Point> > rightcontour;
		vector<Vec4i> righthierarchy;
		findContours(forContour_right, rightcontour, righthierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

		if (contourArea(leftcontour[0]) > 200 && contourArea(rightcontour[0]) > 200)
		{
			vector<Moments> muleft(1);
			Point2f mcleft;
			muleft[0] = moments(leftcontour[0], false);
			mcleft = Point2f(static_cast<float>(muleft[0].m10 / muleft[0].m00), static_cast<float>(muleft[0].m01 / muleft[0].m00));

			vector<Moments> muright(1);
			Point2f mcright;
			muright[0] = moments(rightcontour[0], false);
			mcright = Point2f(static_cast<float>(muright[0].m10 / muright[0].m00), static_cast<float>(muright[0].m01 / muright[0].m00));

			/*mcup = Point2f(mcup.x, mcup.y + _up);
			mcdown = Point2f(mcdown.x, mcdown.y + _down);*/
			mcleft = Point2f(mcleft.x + _left, mcleft.y);
			mcright = Point2f(mcright.x + _right, mcright.y);
			if (mcleft.y == mcright.y)
			{
				//水平
				thisCenter.y = mcleft.y;
				k2 = 0;
				b2 = thisCenter.y;
			}
			else if (mcleft.x != mcright.x)//不可能相等
			{
				k2 = (mcleft.y - mcright.y) / (mcleft.x - mcright.x);
				b2 = (mcleft.y - k2*(mcleft.x));
			}
			else
			{
				return;
			}
		}
		else {
			if (_mode <= 2)
			{
				thisCenter.y = LINE_SRC_ROI_HEIGHT_RESIZE - 0.5*LINE_UD_ROI_HEIGHT;
				k2 = 0;
				b2 = thisCenter.y;
			}
			else {
				thisCenter.y = 0.5*LINE_UD_ROI_HEIGHT;
				k2 = 0;
				b2 = thisCenter.y;
			}
		}
	}
	else {
		if (_mode <= 2)
		{
			thisCenter.y = LINE_SRC_ROI_HEIGHT_RESIZE - 0.5*LINE_UD_ROI_HEIGHT;
			k2 = 0;
			b2 = thisCenter.y;
		}
		else {
			thisCenter.y = 0.5*LINE_UD_ROI_HEIGHT;
			k2 = 0;
			b2 = thisCenter.y;
		}
	}

	if (k1) {
		thisCenter.x = (b2 - b1) / (k1 - k2);
		thisCenter.y = k1*thisCenter.x + b1;
	}
	else {
		thisCenter.y = k2*thisCenter.x + b2;
	}

	deltaX = (Line_Robot_Center.x - thisCenter.x) * Line_KX;
	deltaY = (Line_Robot_Center.y - thisCenter.y) * Line_KY;
	Line_TextOut_Center = Point2f(thisCenter.x, thisCenter.y);
	Line_Order_Center.x = Line_TextOut_Center.x;
	Line_Order_Center.y = Line_TextOut_Center.y;
	OutPutIFM(_IFM, deltaX, deltaY);
	return;
}

int GetLineIFM::InitAngel()
{
	//基于摄像头的竖向线来初始化角度
	Mat forCalculat;
	Line_Line.copyTo(forCalculat);
	float angel = 0;
	Mat up(forCalculat, Rect(0, 0, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT)), down(forCalculat, Rect(0, LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT, LINE_SRC_ROI_WIDETH_RESIZE, LINE_UD_ROI_HEIGHT));
	/*Mat left(forCalculat, Rect(0, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT)), right(forCalculat, Rect(LINE_SRC_ROI_WIDETH - LINE_LR_ROI_WIDETH, 0, LINE_LR_ROI_WIDETH, LINE_SRC_ROI_HEIGHT));*/
	vector<vector<Point> > upcontour;
	vector<Vec4i> uphierarchy;
	findContours(up, upcontour, uphierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
	vector<vector<Point> > downcontour;
	vector<Vec4i> downhierarchy;
	findContours(down, downcontour, downhierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

	//确保兴趣区只有一个轮廓
	//正向放置角度初始化
	int upcontourSize = upcontour.size();
	int downcontourSize = downcontour.size();
	if (upcontourSize == 1 && downcontourSize == 1 )
	{
		vector<Moments> muup(1);
		Point2f mcup;
		muup[0] = moments(upcontour[0], false);
		mcup = Point2f(static_cast<float>(muup[0].m10 / muup[0].m00), static_cast<float>(muup[0].m01 / muup[0].m00));

		vector<Moments> mudown(1);
		Point2f mcdown;
		mudown[0] = moments(downcontour[0], false);
		mcdown = Point2f(static_cast<float>(mudown[0].m10 / mudown[0].m00), static_cast<float>(mudown[0].m01 / mudown[0].m00));

		//竖直线 y=k1*x+b1
		//横线   y=k2*x+b2

		float k1 = 0;
		float b1 = 0;

		if (mcup.x == mcdown.x)
		{
			//竖直情况;
			angel = 0.0;
            Line_Calib_Angel = angel - Line_Calib_Angel;
			return 1;
		}
		else
		{
			//非竖直情况;
			k1 = (mcup.y - (mcdown.y + LINE_SRC_ROI_HEIGHT_RESIZE - LINE_UD_ROI_HEIGHT)) / (mcup.x - mcdown.x);
			b1 = (mcup.y - k1*mcup.x);

			angel = atan(k1) / CV_PI * 180;
			if (angel < 0)
				Line_Calib_Angel = angel + 90.0 /*- startAngel*/;
			else
				Line_Calib_Angel = angel - 90.0 /*- startAngel*/;
			return 1;
		}
	}
	return 0;
}

//return 1;getcenter succeed;
int GetLineIFM::GetCenter()
{
	//Mat dst;
	//grayImage.copyTo(dst);
	//dst = dst > 250;

	//vector<vector<Point> > contours;
	//vector<Vec4i> hierarchy;
	//double area = 0.0;
	//int number = -1;
	//findContours(dst, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
	//int contoursSize = contours.size();
	//for (int i = 0; i < contoursSize; i++)
	//{
	//	double nowarea = contourArea(contours[i]);
	//	if (nowarea > area)
	//	{
	//		//记录面积最大的轮廓的序号
	//		area = nowarea;
	//		number = i;
	//	}
	//}
	//if (number == -1)
	//{
	//	return 0;
	//}
	//else
	//{
	//	vector<Moments> mu(1);
	//	mu[0] = moments(contours[number], false);
	//	center = Point2f(static_cast<float>(mu[0].m10 / mu[0].m00), static_cast<float>(mu[0].m01 / mu[0].m00));
	//	return 1;
	//}
	Line_Robot_Center.x = 115.625;
	Line_Robot_Center.y = 125.642;
	return 1;
}

void GetLineIFM::OutPutIFM(float _IFM[], float Delta_X, float Delta_Y)
{
	if (Line_X_Y_Swap)//XY转换
	{
		switch (Line_LineKind)
		{
		case 1:
			_IFM[1] = Line_X_Contrary*Delta_Y;
			break;
		case 2:
			_IFM[2] = Line_Y_Contrary*Delta_X;
			break;
		case 3:
			_IFM[1] = Line_X_Contrary*Delta_Y;
			_IFM[2] = Line_Y_Contrary*Delta_X;
			break;
		default:
			break;
		}
	}
	else
	{
		switch (Line_LineKind)
		{
		case 1:
			_IFM[2] = Line_Y_Contrary*Delta_Y;
			break;
		case 2:
			_IFM[1] = Line_X_Contrary*Delta_X;
			break;
		case 3:
			_IFM[1] = Line_X_Contrary*Delta_X;
            _IFM[2] = Line_Y_Contrary*Delta_Y;
			break;
		default:
			break;
		}
	}
	return;
}

//CO: 0 关，1 开。  变量Camera_num 代表打开哪个摄像头
int GetLineIFM::CloseOrOpenCamera(int CO, int Camera_num)
{
	if (CO == 0)
	{
		Line_Cap.release();
	}
	else
	{
		Line_Cap.open(Camera_num);
	}
	return 1;
}

